import numpy as np
import matplotlib.pyplot as plt
import copy
import math
import sys
import pathlib
import time
# import driver_model.cubic_spline_planner
import cubic_spline_planner
from Constants import *
from IDM import IDM
# 由Constants.py导入常用常量
from Moveable import Moveable,moveableInitialize,moveableUpdate,lane
from scipy.stats import multivariate_normal
from scipy.spatial import distance
# sys.path.append(str(pathlib.Path(__file__).parent.parent))
sys.path.append('..')
from Dataloader.src import waymo_training_plot_tools as plot_tool
from Dataloader.src import waymo_scenario_classes as class_tool
from Dataloader.src.waymo_transfer_training_to_tf import *
from Dataloader.src.waymo_training_math_tools import *
import tensorflow


# parameter
MAX_T = 100.0  # maximum time to the goal [s]
MIN_T = 5.0  # minimum time to the goal[s]
SIM_LOOP = 500

# Parameter
MAX_SPEED = 50.0 / 3.6  # maximum speed [m/s]
MAX_ACCEL = 2.0  # maximum acceleration [m/ss]
MAX_CURVATURE = 1.0  # maximum curvature [1/m]
MAX_ROAD_WIDTH = 7.0  # maximum road width [m]
D_ROAD_W = 1.0  # road width sampling length [m]
DT = 0.2  # time tick [s]
MAX_T = 5.0  # max prediction time [m]
MIN_T = 4.0  # min prediction time [m]
TARGET_SPEED = 30.0 / 3.6  # target speed [m/s]
D_T_S = 5.0 / 3.6  # target speed sampling length [m/s]
N_S_SAMPLE = 1  # sampling number of target speed
ROBOT_RADIUS = 2.0  # robot radius [m]

# cost weights
K_J = 0.1
K_T = 0.1
K_D = 1.0
K_LAT = 1.0
K_LON = 1.0

show_animation = True

"""
QuinticPolynomial 类 - 用于计算五次多项式系数以及多项式的位置、速度、加速度和 jerk（斜变率）。
quintic_polynomials_planner 函数 - 用于计算机器人的路径，包括位置、速度、加速度和 jerk。
"""

def measure_time(func):
    def wrapper(*args, **kwargs):
        start_time = time.time()
        result = func(*args, **kwargs)
        end_time = time.time()
        execution_time = end_time - start_time
        print(f"函数 {func.__name__} 运行时间：{execution_time:.6f} 秒")
        return result
    return wrapper
class QuinticPolynomial:
    """
    五次多项式类
    xs: 起始点位置
    vxs: 起始点速度
    axs: 起始点加速度
    xe: 终点位置
    vxe: 终点速度
    axe: 终点加速度
    time: 总时间
    """

    def __init__(self, xs, vxs, axs, xe, vxe, axe, time):
        # 输入变道起始点和终点，以及变道时间
        # f = a0 + a1 * t + a2 * t^2 + a3 * t^3 + a4 * t^4 + a5 * t^5,轨迹由五次多项式表示
        # 通过求导得到五次多项式的系数
        # 并提供轨迹方程求导的函数
        self.a0 = xs
        self.a1 = vxs
        self.a2 = axs / 2.0

        A = np.array([[time ** 3, time ** 4, time ** 5],
                      [3 * time ** 2, 4 * time ** 3, 5 * time ** 4],
                      [6 * time, 12 * time ** 2, 20 * time ** 3]])
        print(xe)
        b = np.array([xe - self.a0 - self.a1 * time - self.a2 * time ** 2,
                      vxe - self.a1 - 2 * self.a2 * time,
                      axe - 2 * self.a2])
        x = np.linalg.solve(A, b)   # 返回求解的多项式系数Ax=b

        self.a3 = x[0]
        self.a4 = x[1]
        self.a5 = x[2]

    def calc_point(self, t):
        # calculate the position after time t
        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
            self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5

        return xt

    def calc_first_derivative(self, t):
        # calculate the velocity after time t(即求一阶导)
        xt = self.a1 + 2 * self.a2 * t + \
            3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4

        return xt

    def calc_second_derivative(self, t):
        # calculate the second derivative after time t(即求二阶导)
        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * \
            self.a4 * t ** 2 + 20 * self.a5 * t ** 3

        return xt

    def calc_third_derivative(self, t):
        # calculate the third derivative after time t(即求三阶导)
        xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2

        return xt


def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):
    """
    quintic polynomial planner
    五次规划器
    input
        s_x: start x position [m]
        s_y: start y position [m]
        s_yaw: start yaw angle [rad]
        sa: start accel [m/ss]
        gx: goal x position [m]
        gy: goal y position [m]
        gyaw: goal yaw angle [rad]
        ga: goal accel [m/ss]
        max_accel: maximum accel [m/ss]
        max_jerk: maximum jerk [m/sss]  # 加速度求导（加加速度）
        dt: time tick [s]
    return
        time: time result
        rx: x position result list
        ry: y position result list
        ryaw: yaw angle result list
        rv: velocity result list
        ra: accel result list
    """

    vxs = sv * math.cos(syaw)
    vys = sv * math.sin(syaw)
    vxg = gv * math.cos(gyaw)
    vyg = gv * math.sin(gyaw)

    axs = sa * math.cos(syaw)
    ays = sa * math.sin(syaw)
    axg = ga * math.cos(gyaw)
    ayg = ga * math.sin(gyaw)

    Time, Rx, Ry, Ryaw, Rv, Ra, Rj = [], [], [], [], [], [], []

    for T in np.arange(MIN_T, MAX_T, MIN_T):    # 对于整数，同range;对于实数，同linspace
        # 将x,y方向分开计算
        # 达goal的最短Time到最长Time内的每个T代表一种轨迹
        xqp = QuinticPolynomial(sx, vxs, axs, gx, vxg, axg, T)
        yqp = QuinticPolynomial(sy, vys, ays, gy, vyg, ayg, T)

        time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []

        for t in np.arange(0.0, T + dt, dt):
            # 计算在变道过程中的x,y方向pos,vel,acc,jerk
            # 时间间隔dt
            time.append(t)
            rx.append(xqp.calc_point(t))
            ry.append(yqp.calc_point(t))

            vx = xqp.calc_first_derivative(t)
            vy = yqp.calc_first_derivative(t)
            v = np.hypot(vx, vy)
            yaw = math.atan2(vy, vx)
            rv.append(v)
            ryaw.append(yaw)

            ax = xqp.calc_second_derivative(t)
            ay = yqp.calc_second_derivative(t)
            a = np.hypot(ax, ay)
            if len(rv) >= 2 and rv[-1] - rv[-2] < 0.0:
                a *= -1
            ra.append(a)

            # 计算加加速度
            jx = xqp.calc_third_derivative(t)
            jy = yqp.calc_third_derivative(t)
            j = np.hypot(jx, jy)
            if len(ra) >= 2 and ra[-1] - ra[-2] < 0.0:
                j *= -1
            rj.append(j)

        if max([abs(i) for i in ra]) <= max_accel and max([abs(i) for i in rj]) <= max_jerk:
            # 最大加速度跟最大加加速度不违规
            print("find path!!")
            Time = time
            Rx = rx
            Ry = ry
            Ryaw = ryaw
            Rv = rv
            Ra = ra
            Rj = rj
            break

    # if show_animation:  # pragma: no cover
    #     # show animation:flag，用来显示图画
    #     # 可视化接口将使用一安
    #     for i, _ in enumerate(time):
    #         plt.cla()
    #         # for stopping simulation with the esc key.
    #         plt.gcf().canvas.mpl_connect('key_release_event',
    #                                      lambda event: [exit(0) if event.key == 'escape' else None])
    #         plt.grid(True)
    #         plt.axis("equal")
    #         plot_arrow(sx, sy, syaw)
    #         plot_arrow(gx, gy, gyaw)
    #         plot_arrow(rx[i], ry[i], ryaw[i])
    #         plt.title("Time[s]:" + str(time[i])[0:4] +
    #                   " v[m/s]:" + str(rv[i])[0:4] +
    #                   " a[m/ss]:" + str(ra[i])[0:4] +
    #                   " jerk[m/sss]:" + str(rj[i])[0:4],
    #                   )
    #         plt.pause(0.001)

    return Time, Rx, Ry, Ryaw, Rv, Ra, Rj


def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):  # pragma: no cover
    """
    Plot arrow
    
    """

    if not isinstance(x, float):
        for (ix, iy, iyaw) in zip(x, y, yaw):
            # 将x,y,yaw的list重新组合为tuple，tuple中每个tuple都是(x,y,yaw)
            # 如:((x1,y1,yaw1),(x2,y2,yaw2)...)
            plot_arrow(ix, iy, iyaw)
    else:
        plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
                  fc=fc, ec=ec, head_width=width, head_length=width)
        plt.plot(x, y)




show_animation = True


class QuarticPolynomial:
    """
    四次多项式类
    xs:起始点
    vxs:起始点速度
    axs:起始点加速度
    vxe:终止点速度
    axe:终止点加速度
    time:总耗时
    """
    def __init__(self, xs, vxs, axs, vxe, axe, time):
        # calc coefficient of quartic polynomial

        self.a0 = xs
        self.a1 = vxs
        self.a2 = axs / 2.0

        A = np.array([[3 * time ** 2, 4 * time ** 3],
                      [6 * time, 12 * time ** 2]])
        b = np.array([vxe - self.a1 - 2 * self.a2 * time,
                      axe - 2 * self.a2])
        x = np.linalg.solve(A, b)

        self.a3 = x[0]
        self.a4 = x[1]

    def calc_point(self, t):
        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
            self.a3 * t ** 3 + self.a4 * t ** 4

        return xt

    def calc_first_derivative(self, t):
        xt = self.a1 + 2 * self.a2 * t + \
            3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3

        return xt

    def calc_second_derivative(self, t):
        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2

        return xt

    def calc_third_derivative(self, t):
        xt = 6 * self.a3 + 24 * self.a4 * t

        return xt


class FrenetPath:
    """
    Frenet坐标系下的路径
    """
    def __init__(self):
        self.t = [] # t坐标
        self.d = [] # d坐标
        self.d_d = []
        self.d_dd = []
        self.d_ddd = []
        self.s = [] # s坐标
        self.s_d = []
        self.s_dd = []
        self.s_ddd = []
        self.cd = 0.0
        self.cv = 0.0
        self.cf = 0.0

        self.x = []
        self.y = []
        self.yaw = []
        self.ds = []
        self.c = []


def calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0,Ti,di):
    """_summary_

    Args:
        c_speed (_type_): current speed [m/s]
        c_accel (_type_): current acceleration [m/ss]
        c_d (_type_): current lateral position [m]
        c_d_d (_type_): current lateral speed [m/s]
        c_d_dd (_type_): current lateral acceleration [m/ss]
        s0 (_type_): 车辆起始的s坐标
        di(int):frenet坐标的d方向（横向偏移量）

    Returns:
        _type_: _description_
    """
    # 找到一系列的frenet坐标系下的路径

    # generate path to each offset goal
    # frenet坐标的d方向（横向偏移量）
    # D_ROAD_W为sampling length

    # Lateral（横向） motion planning
    # 对总时长为Ti的路径进行规划
    fp = FrenetPath()   # 用FrenectPath类来存储路径信息

    # lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
    # print("c_d",c_d)
    # print("c_d_dd",c_d_dd)
    # print("di",di)
    lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)    # 对于每个di（横向终点）
    # di是对于每一点横向偏移

    # 计算参数的List
    fp.t = [t for t in np.arange(0.0, Ti, DT)]  # 对Ti，frenet坐标系下的时间
    fp.d = [lat_qp.calc_point(t) for t in fp.t]
    fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
    fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
    fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
    tv = c_speed  # target speed
    # Longitudinal（纵向） motion planning (Velocity keeping)

    # 纵向使用四次多项式进行规划
    lon_qp = QuarticPolynomial(s0, c_speed, c_accel, tv, 0.0, Ti)

    fp.s = [lon_qp.calc_point(t) for t in fp.t]
    fp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
    fp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
    fp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]

    Jp = sum(np.power(fp.d_ddd, 2))  # square of d.jerk
    Js = sum(np.power(fp.s_ddd, 2))  # square of s.jerk

    # square of diff from target speed
    ds = (tv - fp.s_d[-1]) ** 2

    fp.cd = K_J * Jp + K_T * Ti + K_D * fp.d[-1] ** 2
    fp.cv = K_J * Js + K_T * Ti + K_D * ds
    fp.cf = K_LAT * fp.cd + K_LON * fp.cv
    return fp


def calc_global_paths(fp, csp,cx,cy):
    # fplist为frenet坐标系下的路径列表
    # csp为插值后的道路曲线
    # calc global positions
    deltaX = 0
    deltaY = 0
    for i in range(len(fp.s)):
        # print("len",len(fp.s))
        print("fp.s[i]:",fp.s[i])
        ix, iy = csp.calc_position(fp.s[i])
        if ix is None:
            break
        print("ix,iy",ix,iy)
        i_yaw = csp.calc_yaw(fp.s[i])
        print("i_yaw",i_yaw)
        di = fp.d[i]
        fx = ix + di * math.cos(i_yaw + math.pi / 2.0)
        fy = iy + di * math.sin(i_yaw + math.pi / 2.0)
        print("i_yaw:",i_yaw)
        print("di:",di)
        print("**************")
        # print("fx,fy",fx,fy)
        fp.x.append(fx)
        fp.y.append(fy)
    # deltaX = fp.x[0] - cx
    # deltaY = fp.y[0] - cy
    # for i in range(len(fp.x)):
    #     fp.x[i] = fp.x[i] - deltaX
    #     fp.y[i] = fp.y[i] - deltaY

    # calc yaw and ds
    for i in range(len(fp.x) - 1):
        dx = fp.x[i + 1] - fp.x[i]
        dy = fp.y[i + 1] - fp.y[i]
        fp.yaw.append(math.atan2(dy, dx))
        fp.ds.append(math.hypot(dx, dy))

    fp.yaw.append(fp.yaw[-1])
    fp.ds.append(fp.ds[-1])
    print("calc curvature")
    # calc curvature
    for i in range(len(fp.yaw) - 1):
        fp.c.append((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i])

    return fp  # 通过frenet坐标系下的路径列表，计算出全局坐标系下的路径列表


def check_collision(fp, ob):
    # 检测是否会造成碰撞
    for i in range(len(ob[:, 0])):
        d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
             for (ix, iy) in zip(fp.x, fp.y)]

        collision = any([di <= ROBOT_RADIUS ** 2 for di in d])

        if collision:
            return False

    return True


# 定义一个函数，将global坐标系转化为frenet坐标系
def closest_points(x, y, ref_x, ref_y):
    """
    Find the two closest points in the reference path to the car's position.
    """
    distances = [distance.euclidean((x, y), (rx, ry)) for rx, ry in zip(ref_x, ref_y)]
    closest_idx = np.argmin(distances)
    
    if closest_idx == 0:
        next_idx = 1
    elif closest_idx == len(ref_x) - 1:
        next_idx = closest_idx - 1
    else:
        next_idx = closest_idx + 1 if (ref_x[closest_idx + 1] - ref_x[closest_idx]) * (x - ref_x[closest_idx]) + (ref_y[closest_idx + 1] - ref_y[closest_idx]) * (y - ref_y[closest_idx]) > 0 else closest_idx - 1
    
    return closest_idx, next_idx

def frenet_coordinates_improved(car, line):
    """
    Compute the Frenet coordinates (s, d) of the car given its Cartesian coordinates
    and a reference path using the cross product method.
    """
    car_x = car[0]
    car_y = car[1]
    car_theta = car[2]
    ref_x = line[0]
    ref_y = line[1]
    closest_idx, next_idx = closest_points(car_x, car_y, ref_x, ref_y)
    
    # Compute s
    s = sum(np.sqrt((ref_x[i+1] - ref_x[i])**2 + (ref_y[i+1] - ref_y[i])**2) for i in range(closest_idx))
    s += np.sqrt((car_x - ref_x[closest_idx])**2 + (car_y - ref_y[closest_idx])**2)
    
    # Compute d
    dx = ref_x[next_idx] - ref_x[closest_idx]
    dy = ref_y[next_idx] - ref_y[closest_idx]
    
    # Vector from the closest reference point to the car
    vec_to_car = [car_x - ref_x[closest_idx], car_y - ref_y[closest_idx]]
    
    # Cross product
    cross_product = vec_to_car[0] * dy - vec_to_car[1] * dx
    d = np.sqrt(vec_to_car[0]**2 + vec_to_car[1]**2)
    
    if cross_product < 0:
        d = -d
    
    return s, d


def check_paths(fp, ob):
    # 检测路径是否合法
    flag = True
    if any([v > MAX_SPEED for v in fp.s_d]):  # Max speed check
        flag = False
    elif any([abs(a) > MAX_ACCEL for a in
                fp.s_dd]):  # Max accel check
        flag = False
    elif any([abs(c) > MAX_CURVATURE for c in
                fp.c]):  # Max curvature check
        flag = False
    elif ((not ob==None) and (not check_collision(fp, ob))):
        flag = False

    return flag


def frenet_optimal_planning(csp,cx,cy,s0, c_speed, c_accel, c_d, c_d_d, c_d_dd, ob,Ti,di) -> FrenetPath:
    fp = calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0,Ti,di)
    fplist = calc_global_paths(fp, csp,cx,cy)     # 最终要得到的是global坐标系下的路径
    if not check_paths(fp, ob):
        print("get false")
        return False
    else:
        return fp


def generate_target_course(x, y):
    """计算道路样条曲线，xy为道路点的坐标"""
    csp = cubic_spline_planner.CubicSpline2D(x, y)
    # csp是样条曲线的对象
    s = np.arange(0, csp.s[-1], 0.1)

    rx, ry, ryaw, rk = [], [], [], []
    for i_s in s:
        ix, iy = csp.calc_position(i_s)
        # 道路样条曲线上的点
        rx.append(ix)
        ry.append(iy)
        ryaw.append(csp.calc_yaw(i_s))
        rk.append(csp.calc_curvature(i_s))

    return rx, ry, ryaw, rk, csp

class LaneChangeDecision(object):
    """
    车道变换决策
    LEFT = -1
    RIGHT = 1
    提醒：由于轨迹规划的是任意的车道线，因此车道变换决策的时候，不用考虑车辆当前处在什么变道阶段
    """
    def __init__(self, p, db,sigma,T, gap_min=MAIN_SMIN, bsave=MAIN_BSAVE_CAR, bias_right=0):
        '''
        :param p: politeness factor
        :param db: change lane incentive penalty(罚函数)
        :param gap_min: max safe distance
        :param bsave: max safe braking deceleration
        :param bias_right: bias (m/s^2) to drive right
        '''
        self.p, self.db,self.sigma, self.gap_min, self.bsave, self.bias_right = \
                        p, db,sigma, gap_min, bsave , bias_right
        self.T = T
        self.trajectory = {'x':[],'y':[]}
        self.action = 0

    def set_bias_right(self, bias):
        self.bias_right = bias


    def change_ok(self, me: Moveable, f_old: Moveable,
                    f_new1: Moveable, b_new1: Moveable,is_right,b_old: Moveable=None):
        '''
        :param me: the current car
        :param f_old: forward car old
        :param b_old: .. (maybe useless but keep for further use)
        :param f_new1: ..
        :param b_new1: ..
        :return: bool
            change or not
        is 1 if new lane is on the right, else 0
        '''
        # assert isinstance(me, Moveable) and isinstance(f_new1, Moveable)
        # is 1 if new lane is on the right, else 0

        # 距离是否足以变道
        if (me.distance_to(f_new1) <= self.gap_min or
                b_new1.distance_to(me) <= self.gap_min):
            return False

        # 变道后加速度是否处在舒适加速度范围内
        b_new_acc = b_new1.model.calc_acc(b_new1, me)
        me_new_acc = me.model.calc_acc(me, f_new1)
        if (b_new_acc < -self.bsave or me_new_acc < -self.bsave):
            return False

        # 计算变道激励
        me_acc_adv = me_new_acc - me.model.calc_acc(me, f_old) + \
                            is_right * self.bias_right
        b_new_acc_disadv = b_new1.model.calc_acc(b_new1, f_new1) - b_new_acc
        # this means new back car gets more acceleration for my switch
        #   counter-intuitive case...
        if b_new_acc_disadv < 0:
            b_new_acc_disadv = 0
        return me_acc_adv - self.p * b_new_acc_disadv > self.db
    
    def laneChangePlan(self,car,me:Moveable, 
                    f_new1: Moveable, Path:list,f_old: Moveable,b_new1: Moveable,toRight:int,ob:list=None,b_old: Moveable=None)->FrenetPath:
        """是否变道的判断在外部用change_ok函数判断，这里只是计算变道的路径
        Args:
            me (Moveable): _description_
            f_old (Moveable): _description_
            b_old (Moveable): _description_
            f_new1 (Moveable): _description_
            b_new1 (Moveable): _description_
            path_x (_type_): 近距离内的车道线x坐标
            path_y (_type_): _description_
            ob (list): 障碍物x,y坐标list
            toRight=0时go left;else go right
        """
        if(Path==None):
            path = FrenetPath()
            path.x = [0.0,0.0,0.0,0.0,0.0]
            path.y = [0.0,0.1,0.2,0.3,0.4]
            return path
        path_x = Path[0]
        path_y = Path[1]

        rx, ry, ryaw, rk, csp = generate_target_course(path_x,path_y)

        # print(line)
        line = [path_x,path_y]
        s0,d = frenet_coordinates_improved(car,line)
        if((d>0 and toRight == 0) or (d<0 and toRight == 1)):
            d = -d
        print("d",d)
        c_speed = math.sqrt(me.vel[0]**2+me.vel[1]**2)
        c_acc = math.sqrt(me.acc[0]**2+me.acc[1]**2)
        print(c_acc)
        c_d = d
        me.update_d(c_d)
        c_d_d = me.c_d_d
        c_d_dd = me.c_d_dd
        # print(path_x)
        # print(path_y)
        width = 0
        print("c_d:",c_d)
        print("width:",width)
        path = frenet_optimal_planning(csp,me.pos[0],me.pos[1], s0, c_speed,c_acc,c_d ,c_d_d, c_d_dd, ob,self.T,width)

        return path
    
    def getPosDistribution(self,path:FrenetPath):
        """计算车辆未来的位置分布

        Args:
            best_path (FrenetPath): _description_
        """
        pos_index = 0
        for t in path.t:
            if t >0.3:
                break
            pos_index+=1
        pos_index = -1
        pos = np.array(path.x[pos_index],path.y[pos_index])
        cov = np.array([[self.sigma, 0], [0, self.sigma]])
        rv = multivariate_normal(pos, cov)
        return rv.pdf


    #   #源代码demo
    # print(__file__ + " start!!")

    # # way points
    # wx = [0.0, 10.0, 20.5, 35.0, 70.5]
    # wy = [0.0, -6.0, 5.0, 6.5, 0.0]
    # # obstacle lists（障碍物）
    # ob = np.array([[20.0, 10.0],
    #                [30.0, 6.0],
    #                [30.0, 8.0],
    #                [35.0, 8.0],
    #                [50.0, 3.0]
    #                ])

    # tx, ty, tyaw, tc, csp = generate_target_course(wx, wy)

    # # initial state
    # c_speed = 10.0 / 3.6  # current speed [m/s]
    # c_accel = 0.0  # current acceleration [m/ss]
    # c_d = 2.0  # current lateral position [m]
    # c_d_d = 0.0  # current lateral speed [m/s]
    # c_d_dd = 0.0  # current lateral acceleration [m/s]
    # s0 = 0.0  # current course position

    # area = 20.0  # animation area length [m]

    # for i in range(SIM_LOOP):
    #     path = frenet_optimal_planning(
    #         csp, s0, c_speed, c_accel, c_d, c_d_d, c_d_dd, ob)

    #     s0 = path.s[1]
    #     c_d = path.d[1]
    #     c_d_d = path.d_d[1]
    #     c_d_dd = path.d_dd[1]
    #     c_speed = path.s_d[1]
    #     c_accel = path.s_dd[1]

    #     if np.hypot(path.x[1] - tx[-1], path.y[1] - ty[-1]) <= 1.0:
    #         print("Goal")
    #         break

    #     if show_animation:  # pragma: no cover
    #         plt.cla()
    #         # for stopping simulation with the esc key.
    #         plt.gcf().canvas.mpl_connect(
    #             'key_release_event',
    #             lambda event: [exit(0) if event.key == 'escape' else None])
    #         plt.plot(tx, ty)
    #         plt.plot(ob[:, 0], ob[:, 1], "xk")
    #         plt.plot(path.x[1:], path.y[1:], "-or")
    #         plt.plot(path.x[1], path.y[1], "vc")
    #         plt.xlim(path.x[1] - area, path.x[1] + area)
    #         plt.ylim(path.y[1] - area, path.y[1] + area)
    #         plt.title("v[km/h]:" + str(c_speed * 3.6)[0:4])
    #         plt.grid(True)
    #         plt.pause(0.0001)

    # print("Finish")
    # if show_animation:  # pragma: no cover
    #     plt.grid(True)
    #     plt.pause(0.0001)
    #     plt.show()
def main():
    dataset = '/home/mdmp/ppp/dataset/uncompressed_scenario_training_training.tfrecord-00237-of-01000'
    dataloader = Dataloader(dataset)
    # 车辆初始化
    # 初始化自车
    initLane = lane(0,0,0,0,0)
    meId = 2214
    meIDM = IDM(14,10,4,1)
    f_new1IDM = IDM(14,10,4,1)
    f_new2IDM = IDM(14,10,4,1)
    b_new1IDM = IDM(14,10,4,1)
    b_new2IDM = IDM(14,10,4,1)
    me = Moveable(1, [0, 0], [0, 0, 0], initLane, 4.5,4)
    me = moveableInitialize(dataloader,meId,me,meIDM)
    
    idFold = 0
    idBnew = 0
    idFnew = 0
    # 初始化当前前车
    f_old = Moveable(1, [0, 0], [0, 0, 0], initLane, 4.5,4)
    # 初始化向左变道后后车
    b_new1 = Moveable(1, [0, 0], [0, 0, 0], initLane, 4.5,4)
    # 初始化变道后前车
    f_new1 = Moveable(1, [0, 0], [0, 0, 0], initLane, 4.5,4)

    # 初始化向右变道后后车
    b_new2 = Moveable(1, [0, 0], [0, 0, 0], initLane, 4.5,4)
    # 初始化变道后前车
    f_new2 = Moveable(1, [0, 0], [0, 0, 0], initLane, 4.5,4)

    laneChange = LaneChangeDecision(0.8,1,1,5)
    me = moveableUpdate(dataloader,meId,29,me,meIDM)
    print("###########################Initialize#####################################")
    for timeIndex in range(28,71):
        print("###############################time index:",timeIndex,"################################")
        # 车辆信息更新
        id,lf,lb,rf,rb = dataloader.get_around_cars_byID(meId)
        # print(id,lf,lb,rf,rb)
        # print(id)
        # print(lf)
        # print(rb)
        if(len(lf[timeIndex])==0):
            idFnew1 = None
        else:
            idFnew1 = lf[timeIndex][0]
        if(len(rf[timeIndex])==0):
            idFnew2 = None
        else:
            idFnew2 = rf[timeIndex][0]
        if(len(lb[timeIndex])==0):
            idBnew1 = None
        else:
            idBnew1 = lb[timeIndex][0]
        if(len(rb[timeIndex])==0):
            idBnew2 = None
        else:
            idBnew2 = rb[timeIndex][0]
        idFold = id[timeIndex][0]

        if(idFold is None):
            idFold = dataloader.find_independent_car_byID(meId)[timeIndex]
        if(idFnew1 is None):
            idFnew1 = dataloader.find_independent_car_byID(meId)[timeIndex]
        if(idFnew2 is None):
            idFnew2 = dataloader.find_independent_car_byID(meId)[timeIndex]
        if(idBnew1 is None):
            idBnew1 = dataloader.find_independent_car_byID(meId)[timeIndex]
        if(idBnew2 is None):
            idBnew2 = dataloader.find_independent_car_byID(meId)[timeIndex]
        # print("idFold",idFold)
        f_old = moveableUpdate(dataloader,idFold,timeIndex,f_old,1)
        b_new1 = moveableUpdate(dataloader,idBnew1,timeIndex,b_new1,b_new1IDM)
        f_new1 = moveableUpdate(dataloader,idFnew1,timeIndex,f_new1,f_new1IDM)
        print("f_new car's id",idFnew1)
        b_new2 = moveableUpdate(dataloader,idBnew2,timeIndex,b_new2,b_new2IDM)
        f_new2 = moveableUpdate(dataloader,idFnew2,timeIndex,f_new2,f_new2IDM)

        # 预测
        print("id",me.lane.id)
        print("lastId",me.lane.lastId)
        laneleft,laneright =dataloader.get_lane_boundary_byID(me.lane.lastId)
        # print("center:",dataloader.get_center_lane_dict()[me.lane.lastId].point_x_list)
        # print("centerY",dataloader.get_center_lane_dict()[me.lane.lastId].point_y_list)
        # print("laneleft",laneright)
        # if(laneChange.change_ok(me,f_old,f_new1,b_new1,0)):
        if(True):
            # change to the left
            print("意图变道")
            me.lane.id = me.lane.lastId
            print("me.left.id",me.lane.sideId[0])
            print("me.right.id",me.lane.sideId[1])
            # print("laneleft",laneleft)

            # print("here")
            # print("value:",value)
            for pathId in me.lane.sideId[0]:
                path = dataloader.get_long_lane_points_byID(pathId)
                car = (me.pos[0],me.pos[1],dataloader.get_agent_heading_list_byId(meId)[timeIndex])
                now_path = laneChange.laneChangePlan(car,me,f_new1,path[0],path[1],f_old,b_new1,0)
                if(not now_path):
                    continue
                predict_pos = [now_path.x[1],now_path.y[1]]
                width,_ = dataloader.get_agent_distance_to_neibor_lane_byID(meId,timeIndex) # 获取变道距离
                # print(now_path.x)
                # print(now_path.y)
                plot_tool.visualize_on_step(dataset,dataloader,[now_path.x,now_path.y],timeIndex)
                # plot_tool.visualize_on_step(dataset,dataloader,[PathX,PathY],timeIndex)
                print("预测位置：",predict_pos)
                # pdf = laneChange.getPosDistribution(me,f_old,f_new1,b_new1)
        elif(laneChange.change_ok(me,f_old,f_new2,b_new2,1)):
            print("意图右变道")
            now_path = laneChange.laneChangePlan(car,me,f_new2,dataloader.get_lane_boundary_byID(f_new2.lane.lastId),f_new1.lane.path_y,f_old,b_new1,f_new1.lane.sideId[0][0])
            now_path = laneChange.laneChangePlan(me,f_old,f_new2.lane.path_x,f_new2.lane.path_y,f_new2,b_new2,f_new1.lane.sideId[0][1])
            predict_pos = [now_path.x[1],now_path.y[1]]
            plot_tool.visualize_on_step(dataset,dataloader,[now_path.x,now_path.y],timeIndex)
            print("预测位置：",predict_pos)
            pdf = laneChange.getPosDistribution(me,f_old,f_new2,b_new2)
        else:
            print("无变道意图")
        print("and here me dd",me.c_d_d)
        me = moveableUpdate(dataloader,meId,timeIndex+1,me,meIDM)
        print("实际位置：",me.pos)

if __name__ == '__main__':
    main()
